/* Copyright (c) 2010, Iain Wade <iwade@optusnet.com.au>
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *  1. Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *  2. Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *  3. The name of the author may not be used to endorse or promote products
 *     derived from this software without specific prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR
 * IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY DIRECT,
 * INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT,
 * STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 */

#include <sys/param.h>
#include <sys/systm.h>
#include <sys/kernel.h>
#include <sys/module.h>
#include <sys/conf.h>
#include <sys/proc.h>
#include <sys/socket.h>
#include <sys/socketvar.h>
#include <sys/uio.h>
#include <sys/proc.h>
#include <sys/systm.h>
#include <sys/ioccom.h>
#include <sys/conf.h>
#include <sys/bio.h>
#include <geom/geom_disk.h>
#include <netinet/in.h>
#include <sys/malloc.h>
#include <sys/queue.h>
#include <sys/mbuf.h>

#include <sys/ctype.h>

#include "sc101.h"
#include "psan_wireformat.h"

static MALLOC_DEFINE(M_SC101ARR, "sc101 out arr", "sc101 outstanding array");
static MALLOC_DEFINE(M_SC101OUT, "sc101 out", "sc101 outstanding");
static MALLOC_DEFINE(M_SC101IO, "sc101 out io", "sc101 outstanding io");
static MALLOC_DEFINE(M_SC101IOS, "sc101 out ios", "sc101 outstanding io split");
static MALLOC_DEFINE(M_SC101DEV, "sc101 dev", "sc101 device state");
static MALLOC_DEFINE(M_SC101BUF, "sc101 buf", "sc101 internal io buffer");

// initialization and cleanup
static int sc101_init(void);
static void sc101_fini(void);

// attach to a partition, triggered by an ioctl
static int sc101_ioctl(struct cdev *dev, u_long cmd, caddr_t arg, int mode,
                       struct thread *td);
static int sc101_attach(char *id, struct sockaddr_in *broadcast);
static void sc101_attach_complete(struct device_state *dev);
static void sc101_attach_disk(struct device_state *dev);

// perform a disk io for the system
static void sc101_strategy(struct bio *bp);
static void sc101_strategy_completion(struct device_state *dev, int error,
                                      void *ctx);

// packet handling
static int sc101_upcall(struct socket *so, void *arg, int waitflag);
static void handle_packet(struct sockaddr_in *addr, struct mbuf *m, int len);
static void send_packet(struct sockaddr_in *dest, struct mbuf *m,
                        struct outstanding *out);
static void register_packet_handler(struct outstanding *out);
static void unregister_packet_handler(struct outstanding *out);

// helper functions
static uint16_t get_sequence_number(void);
static uint64_t get_uint48(unsigned char *buf);
static int get_next_timeout(int attempt, io_dir_t dir);

// find which root has a partition
static void resolve(struct device_state *dev);
static void handle_resolve_packet(struct sockaddr_in *addr, struct mbuf *m,
                                  size_t len, struct outstanding *out);
static void handle_resolve_timeout(struct outstanding *out);

// find out how many partitions a disk has
static void disk(struct device_state *dev);
static void disk_completion(struct device_state *dev, int error, void *ctx);

// fetch the partition info and match the target id to determine size
static void partition(struct device_state *dev, int partitions);
static void partition_completion(struct device_state *dev, int error,
                                 void *ctx);

// main disk io operations
static void perform_io(struct device_state *dev, struct sockaddr_in *addr,
                       io_dir_t dir, off_t offset, long bcount, caddr_t data,
                       io_complete_cb cb, void *ctx);
static void perform_io_split(struct outstanding_io *io);
static void perform_io_split_completion(struct device_state *dev, int error,
                                        void *ctx);
static void submit_io(struct outstanding_io *io);
static void do_submit_io(struct outstanding_io *io);
static void dequeue_and_submit_io(struct device_state *dev);
static void complete_io(struct outstanding_io *io);
static void handle_io_packet(struct sockaddr_in *addr, struct mbuf *m,
                             size_t len, struct outstanding *out);
static void handle_io_timeout(struct outstanding *out);

static int sc101_load(module_t mod, int cmd, void *arg);

// static void add_timeout(struct outstanding *new_out);
// static void remove_timeout(struct outstanding *out);
// static void update_timeout(void);
// static void process_timeout(void *unused);

// Globals
static struct cdevsw my_devsw = {
  .d_version = D_VERSION,
  .d_ioctl = sc101_ioctl,
  .d_name = "sc101"
};

static struct cdev *ctrl;
static struct socket *so;

static struct device_state_head device_list =
    SLIST_HEAD_INITIALIZER(device_list);
static struct outstanding **outstanding = NULL;
static struct outstanding_head timeout_list =
    TAILQ_HEAD_INITIALIZER(timeout_list);

static int sc101_init() {
  struct thread *td = curthread;
  struct sockaddr_in sin;
  int err;

  if ((err = socreate(AF_INET, &so, SOCK_DGRAM, 0, td->td_ucred, td)) < 0) {
    printf("socreate(): %d\n", err);
    goto fail;
  }

  struct sockopt sopt;
  int one = 1;
  bzero(&sopt, sizeof(sopt));
  sopt.sopt_dir = SOPT_SET;
  sopt.sopt_level = SOL_SOCKET;
  sopt.sopt_name = SO_BROADCAST;
  sopt.sopt_val = &one;
  sopt.sopt_valsize = sizeof(one);
  sosetopt(so, &sopt);

  bzero(&sin, sizeof(sin));
  sin.sin_family = AF_INET;
  sin.sin_len = sizeof(sin);
  sin.sin_port = htons(PSAN_PORT);
  if ((err = sobind(so, (struct sockaddr *)&sin, td)) < 0) {
    sin.sin_port = 0;
    if ((err = sobind(so, (struct sockaddr *)&sin, td)) < 0) {
      printf("sobind(): %d\n", err);
      goto fail;
    }
  }

  soupcall_set(so, SO_RCV, sc101_upcall, ctrl);

  unsigned long size = sizeof(struct outstanding *) * INT16_MAX;
  outstanding = malloc(size, M_SC101ARR, M_WAITOK|M_ZERO);

  ctrl = make_dev(&my_devsw, 0, UID_ROOT, GID_WHEEL, 0600, "sc101/ctrl");

  return 0;

fail:
  if (outstanding)
    free(outstanding, M_SC101ARR);
  if (so)
    soclose(so);

  return EINVAL;
}

static void sc101_fini() {
  for (int i = 0; i < INT16_MAX; i++) {
    if (outstanding[i] == NULL)
      continue;
    free(outstanding[i], M_SC101OUT);
  }

  struct device_state *dev;
  while ((dev = SLIST_FIRST(&device_list))) {
    SLIST_REMOVE_HEAD(&device_list, entries);
    free(dev, M_SC101DEV);
  }

  free(outstanding, M_SC101ARR);
  soclose(so);
  destroy_dev(ctrl);
}

static int sc101_ioctl(struct cdev *dev, u_long cmd, caddr_t arg, int mode,
                       struct thread *td) {
  int err = 0;
  struct proc *procp = td->td_proc;

  printf("sc101_ioctl: dev_t=%d, cmd=%lx, arg=%p, mode=%x procp=%p\n",
         dev2udev(dev), cmd, arg, mode, procp);

  switch(cmd) {
    case SC101_ATTACH: {
      printf("SC101_ATTACH\n");
      struct sc101_attach_req *req = (struct sc101_attach_req *)arg;
      //copyin(arg, &req, sizeof(req));

      struct sockaddr_in broadcast;
      bzero(&broadcast, sizeof(broadcast));
      broadcast.sin_len = sizeof(broadcast);
      broadcast.sin_family = AF_INET;
      broadcast.sin_port = htons(PSAN_PORT);
      broadcast.sin_addr.s_addr = req->broadcast;

      printf("attach %s @ %s\n", &req->id[0], inet_ntoa(broadcast.sin_addr));

      err = sc101_attach(&req->id[0], &broadcast);
      break;
    }
    default:
      printf("No such ioctl for me!\n");
      err = EINVAL;
      break;
  }

  return err;
}

static int sc101_attach(char *id, struct sockaddr_in *broadcast) {
  struct device_state *dev = malloc(sizeof(struct device_state),
                                    M_SC101DEV, M_WAITOK|M_ZERO);
  SLIST_INSERT_HEAD(&device_list, dev, entries);
  mtx_init(&dev->lock, "sc101", "device lock", MTX_DEF);
  memcpy(&dev->id[0], id, sizeof(dev->id));
  memcpy(&dev->bcast_addr, broadcast, sizeof(dev->bcast_addr));
  STAILQ_INIT(&dev->outstanding_io_list);
  STAILQ_INIT(&dev->pending_io_list);

  resolve(dev);

  mtx_lock(&dev->lock);
  msleep(dev, &dev->lock, PWAIT, "sc101", 0);
  mtx_unlock(&dev->lock);

  if (!dev->disk) {
    return ENODEV;
  }

  return 0;
}

static void sc101_attach_complete(struct device_state *dev) {
  mtx_lock(&dev->lock);
  wakeup_one(dev);
  mtx_unlock(&dev->lock);
}

static void sc101_attach_disk(struct device_state *dev) {
  static u_int unit = 0;

  printf("wow, creating unit %d\n", unit);

  dev->disk = disk_alloc();
  dev->disk->d_name = "psan";
  dev->disk->d_unit = unit++;
  dev->disk->d_strategy = sc101_strategy;
  dev->disk->d_sectorsize = 512;
  dev->disk->d_mediasize = dev->size;
  dev->disk->d_maxsize = 1*1024;
  dev->disk->d_drv1 = dev;
  disk_create(dev->disk, DISK_VERSION);
}

static void sc101_strategy(struct bio *bp) {
  struct disk *disk = bp->bio_disk;
  struct device_state *dev = disk->d_drv1;

  switch (bp->bio_cmd) {
    case BIO_READ:
      perform_io(dev, &dev->part_addr, DIR_IN, bp->bio_offset, bp->bio_bcount,
                 bp->bio_data, sc101_strategy_completion, bp);
      break;
    case BIO_WRITE:
      perform_io(dev, &dev->part_addr, DIR_OUT, bp->bio_offset, bp->bio_bcount,
                 bp->bio_data, sc101_strategy_completion, bp);
      break;
    default:
      printf("unknown BIO operation\n");
      biofinish(bp, NULL, EIO);
      return;
  }
}

static void sc101_strategy_completion(struct device_state *dev, int error,
                                      void *ctx) {
  struct bio *bp = (struct bio *)ctx;

  // If partial io complete
  // bp->bio_resid = bp->bio_bcount - donecount;

  if (error) {
    biofinish(bp, NULL, error);
  } else {
    biodone(bp);
  }
}

static int sc101_upcall(struct socket *so, void *arg, int waitflag) {
  printf("sc101_upcall()\n");

  struct sockaddr *addr;
  struct uio uio;
  struct mbuf *m;
  struct mbuf *control;
  int error, rcvflag;

  uio.uio_resid = 1000000000;
  uio.uio_td = curthread;
  do {
    SOCKBUF_UNLOCK(&so->so_rcv);
    addr = NULL;
    m = NULL;
    control = NULL;
    rcvflag = MSG_DONTWAIT;
    error = soreceive(so, &addr, &uio, &m, &control, &rcvflag);
    if (control)
      m_freem(control);
    SOCKBUF_LOCK(&so->so_rcv);

    if (error == EWOULDBLOCK)
      break;

    if (error) {
      printf("soreceive() error %d\n", error);
      break;
    }

    printf("got %d byte packet\n", m_length(m, NULL));
    handle_packet((struct sockaddr_in *)addr, m, m_length(m, NULL));
  } while (m);

  return SU_OK;
}

static void handle_packet(struct sockaddr_in *addr, struct mbuf *m, int len) {
  m = m_pullup(m, sizeof(struct psan_ctrl_t));
  if (!m) {
    printf("short packet, ignoring.\n");
    return;
  }

  struct psan_ctrl_t *ctrl = mtod(m, struct psan_ctrl_t *);
  struct outstanding *out = outstanding[ntohs(ctrl->seq)];

  if (!out || ntohs(ctrl->seq) != out->seq || len != out->len ||
      ctrl->cmd != out->cmd) {
    if (ctrl->cmd == PSAN_ERROR && out && out->timeout_ms) {
      printf("Drive not ready, backing off for %d seconds\n",
             SPINUP_INTERVAL_MS / 1000);
      
      // remove_timeout(out);
      out->timeout_ms = SPINUP_INTERVAL_MS;
      // add_timeout(out);
    } else if (ctrl->cmd != PSAN_FIND && ctrl->cmd != PSAN_RESOLVE) {
      printf("No matching request for seq#%d,cmd=0x%02x,len=%d "
             "expected:cmd=0x%02x,len=%d\n",
             ntohs(ctrl->seq), ctrl->cmd, len,
             out?out->cmd:0, out?out->len:-1);    
    }

    m_freem(m);
    
    return;
  }

  unregister_packet_handler(out);
  
  printf("calling packet handler\n");
  out->packet_handler(addr, m, len, out);
}

static void send_packet(struct sockaddr_in *dest, struct mbuf *m,
                        struct outstanding *out) {
  if (out)
    register_packet_handler(out);

  printf("sending %d byte packet\n", m_length(m, NULL));

  int err = 0;
  if ((err = sosend(so, (struct sockaddr *)dest, NULL, m, NULL, 0,
                    curthread)) < 0) {
    printf("sosend(): %d\n", err);
    return;
  }
}

static void register_packet_handler(struct outstanding *out) {
  if (outstanding[out->seq] != NULL)
    printf("seq#%d already used!\n", out->seq);

  outstanding[out->seq] = out;

  // if (out->timeout_ms)
  //   add_timeout(out);
}

static void unregister_packet_handler(struct outstanding *out) {
  outstanding[out->seq] = NULL;

  // if (out->timeout_ms)
  //   remove_timeout(out);
}

static uint16_t get_sequence_number() {
  static uint16_t seq = 0;
  if (seq >= INT16_MAX)
    seq = 0;
  return htons(seq++);
}

static uint64_t get_uint48(unsigned char *buf) {
  // TODO(iwade) endianess?
  uint64_t ret = 0;
  for (int i = 0; i < 6; i++)
    ret = (ret << 8) | buf[i];
  return ret;
}

static int get_next_timeout(int attempt, io_dir_t dir) {
  static struct {
    int timeout_ms;
    int tries;
  } retry_plan[] = {
    {  500,  1 },
    { 1000,  1 },
    { 3000, 20 },
    {    0,  0 }
  };

  for (int i = 0, a = 0; retry_plan[i].tries; i++) {
    a += retry_plan[i].tries;

    if (attempt >= a)
      continue;

    return retry_plan[i].timeout_ms;
  }

  // retry writes indefinitely.
  if (dir == DIR_OUT) {
    return 10000;
  }

  return 0;
}

static void resolve(struct device_state *dev) {
  printf("resolve\n");
  
  dev->last_resolve = time_uptime;

  struct psan_resolve_t req;
  bzero(&req, sizeof(req));
  req.ctrl.cmd = PSAN_RESOLVE;
  req.ctrl.seq = get_sequence_number();
  memcpy(req.id, dev->id, sizeof(req.id));
  
  struct mbuf *m = m_getcl(M_WAIT, MT_DATA, M_PKTHDR);
  m_copyback(m, 0, sizeof(req), (caddr_t)&req);

  struct outstanding *out = malloc(sizeof(struct outstanding),
                                   M_SC101OUT, M_WAITOK|M_ZERO);
  out->seq = ntohs(req.ctrl.seq);
  out->len = sizeof(struct psan_resolve_response_t);
  out->cmd = PSAN_RESOLVE_RESPONSE;
  out->timeout_ms = RESOLVE_TIMEOUT_MS;
  out->timeout = 0;
  out->packet_handler = handle_resolve_packet;
  out->timeout_handler = handle_resolve_timeout;
  out->ctx = dev;
  
  send_packet(&dev->bcast_addr, m, out);
}

static void handle_resolve_packet(struct sockaddr_in *addr, struct mbuf *m,
                                  size_t len, struct outstanding *out) {
  printf("resolve succeeded!\n");
  
  struct device_state *dev = (struct device_state *)out->ctx;

  dev->last_reply = time_uptime;
  
  m = m_pullup(m, out->len);
  if (!m) {
    printf("m_pullup failed\n");
    return;
  }

  struct psan_resolve_response_t *res =
      mtod(m, struct psan_resolve_response_t *);
  
  memcpy(&dev->root_addr, addr, sizeof(dev->root_addr));

  struct sockaddr_in part;
  bzero(&part, sizeof(part));
  part.sin_len = sizeof(part);
  part.sin_family = AF_INET;
  part.sin_port = htons(PSAN_PORT);
  part.sin_addr = res->ip4;
  
  memcpy(&dev->part_addr, &part, sizeof(dev->part_addr));

  free(out, M_SC101OUT);
  m_free(m);

  if (!dev->disk) {
    disk(dev);
  }
}

static void handle_resolve_timeout(struct outstanding *out) {
  struct device_state *dev = (struct device_state *)out->ctx;

  free(out, M_SC101OUT);

  if (!dev->disk) {
    sc101_attach_complete(dev);
  }
}

static void disk(struct device_state *dev) {
  printf("disk\n");

  off_t offset = 0;
  long bcount = 1;
  caddr_t data = malloc(bcount * SECTOR_SIZE, M_SC101BUF, M_WAITOK|M_ZERO);

  perform_io(dev, &dev->root_addr, DIR_IN, offset, bcount, data,
             disk_completion, data);
}

static void disk_completion(struct device_state *dev, int error, void *ctx) {
  printf("disk complete: %d\n", error);

  struct psan_get_response_disk_t *disk =
      (struct psan_get_response_disk_t *)ctx;
  int partitions = disk->partitions;
  free(disk, M_SC101BUF);

  if (error) {
    sc101_attach_complete(dev);
    return;
  }

  partition(dev, partitions);
}

static void partition(struct device_state *dev, int partitions) {
  printf("partition\n");

  off_t offset = 1 * SECTOR_SIZE;
  long bcount = partitions;
  caddr_t data = malloc(bcount * SECTOR_SIZE, M_SC101BUF, M_WAITOK|M_ZERO);

  perform_io(dev, &dev->root_addr, DIR_IN, offset, bcount, data,
             partition_completion, data);
}

static void partition_completion(struct device_state *dev, int error,
                                 void *ctx) {
  printf("partition complete: %d\n", error);

  if (error) {
    free(ctx, M_SC101BUF);
    sc101_attach_complete(dev);
    return;
  }

  // TODO(iwade) need buffer length to determine number of parts in response.
  int n = 1;

  for (int i = 0; i < n; i++) {
    struct psan_get_response_partition_t *part = ctx;
    //    (struct psan_get_response_partition_t *)&data[i * sizeof(*part)];

    if (memcmp(&dev->id[0], &part->id[0], sizeof(dev->id)) != 0) {
      char buf[sizeof(part->id) + 1];
      memcmp(&buf[0], &part->id[0], sizeof(part->id));
      buf[sizeof(buf)-1] = 0;
      printf("no match: %s\n", buf);
      continue;
    }

    printf("matched partition\n");
    memcpy(dev->label, part->label, sizeof(dev->label));
    dev->size = get_uint48(part->sector_size) * SECTOR_SIZE;

    sc101_attach_disk(dev);
    break;
  }

  free(ctx, M_SC101BUF);
  sc101_attach_complete(dev);
}

static void perform_io(struct device_state *dev, struct sockaddr_in *addr,
                       io_dir_t dir, off_t offset, long bcount, caddr_t data,
                       io_complete_cb cb, void *ctx) {
  printf("IO: %d blocks @ %d\n", (int)bcount, (int)(offset/512));

  struct outstanding_io *io = malloc(sizeof(struct outstanding_io),
                                     M_SC101IO, M_WAITOK|M_ZERO);
  io->dev = dev;
  io->addr = addr;
  io->dir = dir;
  io->offset = offset;
  io->bcount = bcount;
  io->data = data;
  io->cb = cb;
  io->ctx = ctx;
  io->attempt = 0;

  if ((bcount * SECTOR_SIZE) > MAX_IO_SIZE || bcount & (bcount - 1)) {
    perform_io_split(io);
    return;
  }

  mtx_lock(&dev->lock);
  submit_io(io);
  mtx_unlock(&dev->lock);
}

static void perform_io_split(struct outstanding_io *io) {
  printf("odd sized io, split\n");

  struct outstanding_io_split *split =
      malloc(sizeof(struct outstanding_io_split), M_SC101IOS, M_WAITOK|M_ZERO);
  mtx_init(&split->lock, "sc101", "split lock", MTX_DEF);
  split->io = io;

  long io_len = io->bcount * SECTOR_SIZE;

  mtx_lock(&split->lock);
  for (off_t used = 0, use = min(MSB(io_len), MAX_IO_SIZE);
       used < io_len;
       used += use, use = min(MSB(io_len - used), MAX_IO_SIZE)) {
    split->pending++;

    perform_io(io->dev, io->addr, io->dir, io->offset + used, use / SECTOR_SIZE,
               io->data + used, perform_io_split_completion, io);
  }
  mtx_unlock(&split->lock);
}

static void perform_io_split_completion(struct device_state *dev, int error,
                                        void *ctx) {
  printf("split io complete: %d\n", error);

  struct outstanding_io_split *split = (struct outstanding_io_split *)ctx;

  mtx_lock(&split->lock);
  if (!split->error)
    split->error = error;
  split->pending--;
  if (!split->pending) {
    printf("all split parts complete\n");

    struct outstanding_io *io = split->io;
    if (io->cb)
      io->cb(io->dev, split->error, io->ctx);
    free(io, M_SC101IO);
    free(split, M_SC101IOS);
  }
  mtx_unlock(&split->lock);
}

static void submit_io(struct outstanding_io *io) {
  struct device_state *dev = io->dev;

  mtx_assert(&dev->lock, MA_OWNED);
  if (dev->outstanding_io_count < MAX_OUTSTANDING_IO) {
    printf("sending\n");

    STAILQ_INSERT_TAIL(&io->dev->outstanding_io_list, io, entries);
    io->dev->outstanding_io_count++;
    do_submit_io(io);
  } else {
    printf("queueing\n");

    STAILQ_INSERT_TAIL(&io->dev->pending_io_list, io, entries);
    io->dev->pending_io_count++;
  }
}

static void do_submit_io(struct outstanding_io *io) {
  long io_len = io->bcount * SECTOR_SIZE;
  struct mbuf *m = NULL;

  m = m_getcl(M_WAIT, MT_DATA, M_PKTHDR);
  if (!m) {
    printf("mbuf creation failed\n");
    return;
  }

  switch (io->dir) {
    case DIR_IN: {
      struct psan_get_t req;
      bzero(&req, sizeof(req));
      req.ctrl.cmd = PSAN_GET;
      req.ctrl.seq = get_sequence_number();
      req.ctrl.len_power = POWER_OF_2(io_len);
      req.sector = htonl(io->offset / SECTOR_SIZE);
    
      m_copyback(m, 0, sizeof(req), (caddr_t)&req);

      io->outstanding.seq = ntohs(req.ctrl.seq);
      io->outstanding.len = sizeof(struct psan_get_response_t) + io_len;
      io->outstanding.cmd = PSAN_GET_RESPONSE;

      break;
    }
    case DIR_OUT: {
      struct psan_put_t req;
      bzero(&req, sizeof(req));
      req.ctrl.cmd = PSAN_PUT;
      req.ctrl.seq = get_sequence_number();
      req.ctrl.len_power = POWER_OF_2(io_len);
      req.sector = htonl(io->offset / SECTOR_SIZE);

      m_copyback(m, 0, sizeof(req), (caddr_t)&req);
      m_copyback(m, sizeof(req), io_len, io->data);

      io->outstanding.seq = ntohs(req.ctrl.seq);
      io->outstanding.len = sizeof(struct psan_put_response_t);
      io->outstanding.cmd = PSAN_PUT_RESPONSE;

      break;
    }
  }

  io->outstanding.packet_handler = handle_io_packet;
  io->outstanding.timeout_handler = handle_io_timeout;
  io->outstanding.ctx = io;
  io->outstanding.timeout_ms = io->timeout_ms;

  send_packet(io->addr, m, &io->outstanding);
}

static void dequeue_and_submit_io(struct device_state *dev) {
  mtx_assert(&dev->lock, MA_OWNED);
  struct outstanding_io *io = STAILQ_FIRST(&dev->pending_io_list);
  if (io) {
    STAILQ_REMOVE(&dev->pending_io_list, io, outstanding_io, entries);
    dev->pending_io_count--;
    submit_io(io);
  }
}

static void complete_io(struct outstanding_io *io) {
  struct device_state *dev = io->dev;

  mtx_assert(&dev->lock, MA_OWNED);
  STAILQ_REMOVE(&dev->outstanding_io_list, io, outstanding_io, entries);
  dev->outstanding_io_count--;
  dequeue_and_submit_io(dev);
}

static void handle_io_packet(struct sockaddr_in *addr, struct mbuf *m,
                             size_t len, struct outstanding *out) {
  printf("handle_io_packet\n");

  struct outstanding_io *io = (struct outstanding_io *)out->ctx;
  struct device_state *dev = io->dev;

  io->dev->last_reply = time_uptime;

  if (io->dir == DIR_IN) {
    printf("copying data in\n");
    m_copydata(m, sizeof(struct psan_get_response_t),
               len - sizeof(struct psan_get_response_t), io->data);
  }

  printf("free m\n");
  m_free(m);

  if (io->cb) {
    printf("calling cb\n");
    io->cb(io->dev, 0, io->ctx);
  }

  printf("complete io\n");
  mtx_lock(&dev->lock);
  complete_io(io);
  mtx_unlock(&dev->lock);

  printf("free\n");
  free(io, M_SC101IO);
}

static void handle_io_timeout(struct outstanding *out) {
  struct outstanding_io *io = (struct outstanding_io *)out->ctx;
  struct device_state *dev = io->dev;

  io->attempt++;
  io->timeout_ms = get_next_timeout(io->attempt, io->dir);

  if (io->timeout_ms) {
    if (io->attempt > 3) {
      printf("retry IO (%p, %d, %d)\n", io, io->attempt, io->timeout_ms);
    }
    do_submit_io(io);
    return;
  }

  printf("abort IO %p\n", io);

  if (io->cb)
    io->cb(io->dev, ETIMEDOUT, io->ctx);

  mtx_lock(&dev->lock);
  complete_io(io);
  mtx_unlock(&dev->lock);

  free(io, M_SC101IO);
}

static int sc101_load(module_t mod, int cmd, void *arg) {
  int err = 0;

  switch (cmd) {
    case MOD_LOAD:
      printf("SC101/T driver loaded.\n");
      printf("Copyright (c) 2010 Iain Wade <iwade@optusnet.com.au>\n");
      err = sc101_init();
      break;
    case MOD_UNLOAD:
      printf("Unloaded SC101/T device driver.\n");
      sc101_fini();
      break;
    default:
      err = EOPNOTSUPP;
      break;
  }

  return err;
}

DEV_MODULE(cdev, sc101_load, NULL);



/*


static void add_timeout(struct outstanding *new_out) {
  mtx_lock(&timeout_mutex);

  new_out->timeout = ticks + hz * (out->timeout_ms / 1000);

  struct outstanding *out = NULL;
  TAILQ_FOREACH_REVERSE(out, &timeout_list, outstanding_head, entries)
    if (out->timeout < new_out->timeout)
      break;
  
  if (out)
    TAILQ_INSERT_AFTER(&timeout_list, out, new_out, entries);
  else
    TAILQ_INSERT_HEAD(&timeout_list, new_out, entries);
  
  if (new_out == TAILQ_FIRST(&timeout_list))
    update_timeout();

  mtx_unlock(&timeout_mutex);
}

static void remove_timeout(struct outstanding *out) {
  mtx_lock(&timeout_mutex);

  int update = 0;

  if (out == TAILQ_FIRST(&timeout_list))
    update = 1;

  TAILQ_REMOVE(&timeout_list, out, entries);

  if (update)
    update_timeout();

  mtx_unlock(&timeout_mutex);
}

static void update_timeout() {
  mtx_assert(&timeout_mutex, MA_OWNED);

  struct outstanding *out = TAILQ_FIRST(&timeout_list);
  
  if (out)
    callout_schedule(&timeout_callout, ticks - out->timeout);
  else
    callout_stop(&timeout_callout);
}

static void process_timeout(void *unused) {
  mtx_assert(&timeout_mutex, MA_OWNED);
  struct outstanding *out = TAILQ_FIRST(&timeout_list);

  if (out)
  {
    unregister_packet_handler(out);
    out->timeout_handler(out, out->ctx);
  }
}



static struct mtx timeout_mutex;
mtx_init(&timeout_mutex, "sc101", "timeout", MTX_DEF);

static struct callout_handle callout;

  // 

  // initialize the callout with a mutex to be held around all calls.
  callout_init_mtx(&callout, &timeout_mutex, 0);

  // spurious timeout, needed to setup the callback func
  callout_reset(&callout, 1, process_timeout, NULL);


*/
